/****************************************************************
 * Fulguro
 * Copyright (C) 2004 Christophe Clienti
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 ***************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef __SSE2__
#include <xmmintrin.h>
#include <emmintrin.h>
#endif

#include <flgrCoreDispatch.h>

#include "flgrMeasureBaseSSE2.h"



void flgr1d_measure_min_max_spp1_SSE2_fgUINT8(FLGR_Data1D *dat, FLGR_Vector *mini, FLGR_Vector *maxi) {
#ifdef __SSE2__
  fgUINT8 array_min[16] __attribute__ ((aligned (16)));
  fgUINT8 array_max[16] __attribute__ ((aligned (16)));
  fgUINT8 _mini, _maxi;
  fgUINT8 *psrc = (fgUINT8*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rmax,rmin;
  int i, nbvector = dat->length>>4;

  FLGR_DECLARE_FUNCTION;

  rmin = vec_psrc[0];
  rmax = vec_psrc[0];

  for(i=1 ; i<nbvector ; i++) {
    rmin = _mm_min_epu8(rmin,vec_psrc[i]);
    rmax = _mm_max_epu8(rmax,vec_psrc[i]);
  }

  *((__m128i*) array_min) = rmin;
  *((__m128i*) array_max) = rmax;

  _mini = array_min[0];
  _maxi = array_max[0];

  for(i=1 ; i<16 ; i++) {
    _mini = FLGR_MIN(_mini,array_min[i]);
    _maxi = FLGR_MAX(_maxi,array_max[i]);
  }

  for(i=nbvector<<4 ; i<dat->length ; i++) {
    _mini = FLGR_MIN(psrc[i],_mini);
    _maxi = FLGR_MAX(psrc[i],_maxi);
  }

  *((fgUINT8*) mini->array) = _mini;
  *((fgUINT8*) maxi->array) = _maxi;
#endif
}


void flgr1d_measure_min_max_spp1_SSE2_fgINT16(FLGR_Data1D *dat, FLGR_Vector *mini, FLGR_Vector *maxi) {
#ifdef __SSE2__
  fgINT16 array_min[8] __attribute__ ((aligned (16)));
  fgINT16 array_max[8] __attribute__ ((aligned (16)));
  fgINT16 _mini, _maxi;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rmax,rmin;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;

  rmin = vec_psrc[0];
  rmax = vec_psrc[0];

  for(i=1 ; i<nbvector ; i++) {
    rmin = _mm_min_epi16(rmin,vec_psrc[i]);
    rmax = _mm_max_epi16(rmax,vec_psrc[i]);
  }

  *((__m128i*) array_min) = rmin;
  *((__m128i*) array_max) = rmax;

  _mini = array_min[0];
  _maxi = array_max[0];

  for(i=1 ; i<8 ; i++) {
    _mini = FLGR_MIN(_mini,array_min[i]);
    _maxi = FLGR_MAX(_maxi,array_max[i]);
  }

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _mini = FLGR_MIN(psrc[i],_mini);
    _maxi = FLGR_MAX(psrc[i],_maxi);
  }

  *((fgINT16*) mini->array) = _mini;
  *((fgINT16*) maxi->array) = _maxi;
#endif
}



void flgr1d_measure_volume_u32_spp1_SSE2_fgUINT8(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT32 array_vol[4] __attribute__ ((aligned (16)));
  fgUINT32 _volume=0;
  fgUINT8 *psrc = (fgUINT8*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i v1, v2;
  __m128i v3;
  __m128i vec_zero;
  int i, nbvector = dat->length>>4;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0

  rvol = vec_zero;


  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];
    v1 = _mm_unpackhi_epi8(v2,vec_zero);
    v2 = _mm_unpacklo_epi8(v2,vec_zero);

    v3 = _mm_unpackhi_epi16(v1,vec_zero);
    v1 = _mm_unpacklo_epi16(v1,vec_zero);
    v1 = _mm_add_epi32(v1,v3);

    v3 = _mm_unpackhi_epi16(v2,vec_zero);
    v2 = _mm_unpacklo_epi16(v2,vec_zero);
    v2 = _mm_add_epi32(v2,v3);

    v1 = _mm_add_epi32(v1,v2);

    rvol = _mm_add_epi32(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  for(i=0 ; i<4 ; i++) {
    _volume += array_vol[i];
  }

  for(i=nbvector<<4 ; i<dat->length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}



void flgr1d_measure_volume_u64_spp1_SSE2_fgUINT8(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT64 array_vol[2] __attribute__ ((aligned (16)));
  fgUINT64 _volume;
  fgUINT8 *psrc = (fgUINT8*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i rsrc;
  __m128i v1, v2;
  __m128i v1_1, v1_2;
  __m128i v2_1, v2_2;
  __m128i vt1, vt2;
  __m128i vec_zero;
  int i, nbvector = dat->length>>4;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0
  

  rvol = vec_zero;


  for(i=0 ; i<nbvector ; i++) {
    rsrc = vec_psrc[i];
    v1 = _mm_unpackhi_epi8(rsrc,vec_zero);
    v2 = _mm_unpacklo_epi8(rsrc,vec_zero);

    v1_1 = _mm_unpackhi_epi16(v1,vec_zero);
    v1_2 = _mm_unpacklo_epi16(v1,vec_zero);

    v2_1 = _mm_unpackhi_epi16(v2,vec_zero);
    v2_2 = _mm_unpacklo_epi16(v2,vec_zero);

    vt1  = _mm_unpackhi_epi32(v1_1,vec_zero);
    vt2  = _mm_unpacklo_epi32(v1_1,vec_zero);
    v1_1 = _mm_add_epi64(vt1,vt2);

    vt1  = _mm_unpackhi_epi32(v1_2,vec_zero);
    vt2  = _mm_unpacklo_epi32(v1_2,vec_zero);
    v1_2 = _mm_add_epi64(vt1,vt2);

    vt1  = _mm_unpackhi_epi32(v2_1,vec_zero);
    vt2  = _mm_unpacklo_epi32(v2_1,vec_zero);
    v2_1 = _mm_add_epi64(vt1,vt2);

    vt1  = _mm_unpackhi_epi32(v2_2,vec_zero);
    vt2  = _mm_unpacklo_epi32(v2_2,vec_zero);
    v2_2 = _mm_add_epi64(vt1,vt2);

    v1 = _mm_add_epi64(v1_1,v1_2);
    v2 = _mm_add_epi64(v2_1,v2_2);

    v1 = _mm_add_epi64(v1,v2);

    rvol = _mm_add_epi64(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0]+array_vol[1];

  for(i=nbvector<<4 ; i<dat->length ; i++) {
    _volume += (fgUINT64) psrc[i];
  }

  *((fgUINT64*) volume->array) = _volume;
#endif
}
















void flgr1d_measure_volume_u32_spp1_SSE2_fgUINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT32 array_vol[4] __attribute__ ((aligned (16)));
  fgUINT32 _volume=0;
  fgUINT16 *psrc = (fgUINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i rsrc;
  __m128i v1, v2;
  __m128i vec_zero;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0
  

  rvol = vec_zero;


  for(i=0 ; i<nbvector ; i++) {
    rsrc = vec_psrc[i];
    v1 = _mm_unpackhi_epi16(rsrc,vec_zero);
    v2 = _mm_unpacklo_epi16(rsrc,vec_zero);

    v1 = _mm_add_epi32(v1,v2);

    rvol = _mm_add_epi32(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0];
  _volume += array_vol[1];
  _volume += array_vol[2];
  _volume += array_vol[3];

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}



void flgr1d_measure_volume_u64_spp1_SSE2_fgUINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT64 array_vol[2] __attribute__ ((aligned (16)));
  fgUINT64 _volume;
  fgUINT16 *psrc = (fgUINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i v1, v2;
  __m128i v3;
  __m128i vec_zero;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0
  
  rvol = vec_zero;

  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];
    v1 = _mm_unpackhi_epi16(v2,vec_zero);
    v2 = _mm_unpacklo_epi16(v2,vec_zero);

    v3 = _mm_unpackhi_epi32(v1,vec_zero);
    v1 = _mm_unpacklo_epi32(v1,vec_zero);
    v1 = _mm_add_epi64(v1,v3);

    v3 = _mm_unpackhi_epi32(v2,vec_zero);
    v2 = _mm_unpacklo_epi32(v2,vec_zero);
    v2 = _mm_add_epi64(v2,v3);

    v1 = _mm_add_epi64(v1,v2);

    rvol = _mm_add_epi64(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0]+array_vol[1];

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _volume += (fgUINT64) psrc[i];
  }

  *((fgUINT64*) volume->array) = _volume;
#endif
}








void flgr1d_measure_volume_u32_spp1_SSE2_fgINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT32 array_vol[4] __attribute__ ((aligned (16)));
  fgUINT32 _volume=0;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i rsrc;
  __m128i v1, v2;
  __m128i vec_zero;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0
  

  rvol = vec_zero;


  for(i=0 ; i<nbvector ; i++) {
    rsrc = vec_psrc[i];
    v1 = _mm_unpackhi_epi16(rsrc,vec_zero);
    v2 = _mm_unpacklo_epi16(rsrc,vec_zero);

    v1 = _mm_add_epi32(v1,v2);

    rvol = _mm_add_epi32(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0];
  _volume += array_vol[1];
  _volume += array_vol[2];
  _volume += array_vol[3];

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}






void flgr1d_measure_volume_s32_spp1_SSE2_fgINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgINT32 array_vol[4] __attribute__ ((aligned (16)));
  fgINT32 _volume=0;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i v1, v2;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;


  v2 = vec_psrc[0]; //just to not let v1 initialized
  rvol = _mm_xor_si128( v2, v2 ); //vec_zero = 0


  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];

    v1 = _mm_unpackhi_epi16(v2,v2);
    v1 = _mm_srai_epi32( v1, 16 ); 

    v2 = _mm_unpacklo_epi16(v2,v2);
    v2 = _mm_srai_epi32( v2, 16 ); 

    v1 = _mm_add_epi32(v1,v2);

    rvol = _mm_add_epi32(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0];
  _volume += array_vol[1];
  _volume += array_vol[2];
  _volume += array_vol[3];

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _volume += (fgINT32) psrc[i];
  }

  *((fgINT32*) volume->array) = _volume;
#endif
}

void flgr1d_measure_volume_u64_spp1_SSE2_fgINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE2__
  fgUINT64 array_vol[2] __attribute__ ((aligned (16)));
  fgUINT64 _volume;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m128i *vec_psrc = (__m128i*) dat->array;
  __m128i rvol;
  __m128i v1, v2;
  __m128i v3;
  __m128i vec_zero;
  int i, nbvector = dat->length>>3;

  FLGR_DECLARE_FUNCTION;

  v1=vec_psrc[0]; //just to not let v1 initialized
  vec_zero = _mm_xor_si128( v1, v1 ); //vec_zero = 0
  
  rvol = vec_zero;

  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];
    v1 = _mm_unpackhi_epi16(v2,vec_zero);
    v2 = _mm_unpacklo_epi16(v2,vec_zero);

    v3 = _mm_unpackhi_epi32(v1,vec_zero);
    v1 = _mm_unpacklo_epi32(v1,vec_zero);
    v1 = _mm_add_epi64(v1,v3);

    v3 = _mm_unpackhi_epi32(v2,vec_zero);
    v2 = _mm_unpacklo_epi32(v2,vec_zero);
    v2 = _mm_add_epi64(v2,v3);

    v1 = _mm_add_epi64(v1,v2);

    rvol = _mm_add_epi64(v1,rvol);
  }

  *((__m128i*) array_vol) = rvol;

  _volume = array_vol[0]+array_vol[1];

  for(i=nbvector<<3 ; i<dat->length ; i++) {
    _volume += (fgUINT64) psrc[i];
  }

  *((fgUINT64*) volume->array) = _volume;
#endif
}
